COMMENT ⊗   VALID 00002 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	 POLYA, POLYB
C00010 ENDMK
C⊗;
COMMENT  POLYA, POLYB;

COMMENT: A trajectory is to be calculated.  It is composed of #SEGS
segments, each to be a third degree poly- nomial.  At each point, 0
to #SEGS, there is a given location in LOCS[0:#SEGS], except for
points UNC1 and UNC2, the unconstrained points.  The velocities and
accelerations of the first and last points are also given: these are
FST_VEL, LST_VEL, FST_ACC, and LST_ACC.  The calculation depends on
the array TAU[1:#SEGS], which holds the time for each segment.
Polynomial coefficients are returned in COEFFS[1:N] for the entire
piece: each segment has four coefficients (everything is restricted
to third degree).  The way to have this all work is to call POLYA
once, and then call POLYB for each joint.  ;

PROCEDURE POLYA (SAFE REAL ARRAY A; INTEGER #SEGS; INTEGER ARRAY TAU;
    INTEGER UNC1, UNC2);

	BEGIN "polya"
	DEFINE MEM(ARG) "<>" = <MEMORY[ARG,REAL]>;
	COMMENT:  A is the large matrix.  We will solve Ax=B;
	INTEGER ROW, COL, SEG, N, ALOC;
	REAL TEMP;
	ARRCLR(A);
	N ← 4 * #SEGS;
	UNC1 ← UNC1 + 1;
	UNC2 ← UNC2 + 1;
	ROW ← COL ← 1;
	SEG ← 1;

	COMMENT:  Fill the matrix A for the first segment;
	ALOC ← LOCATION(A[1,1]);
	A[ROW,COL] ← A[ROW+1,COL+1] ← 1.;
	A[ROW+2,COL+2] ← 2.;
	ROW ← ROW + 3;
	COL ← COL + 4;
	ALOC ← ALOC + 3*N + 4;

	FOR SEG ← 2 STEP 1 UNTIL #SEGS DO
		IF SEG=UNC1 ∨ SEG=UNC2 
			THEN BEGIN  "UNCNST" COMMENT:  Left of segment
			is an unconstrained point;
			MEM(ALOC) ← 1.;
			MEM(ALOC-4) ← MEM(ALOC-3) ← MEM(ALOC-2) ← MEM(ALOC-1) ← 
				-1;
			ALOC ← ALOC + N;
			MEM(ALOC-3) ← -1;
			MEM(ALOC-2) ← -2;
			MEM(ALOC-1) ← -3;
			MEM(ALOC+1) ← TEMP ← TAU[SEG-1]/TAU[SEG];
			ALOC ← ALOC + N;
			MEM(ALOC+2) ← TEMP*TEMP;
			MEM(ALOC-1) ← -3;
			MEM(ALOC-2) ← -1;
			COMMENT: same effect as:
				A[ROW,COL] ← 1.
				A[ROW,COL-1] ← A[ROW,COL-2] ← A[ROW,COL-3]
					← A[ROW,COL-4] ← A[ROW+1,COL-3]
					← A[ROW+2,COL-2] ← -1.
				A[ROW+1,COL-1] ← A[ROW+2,COL-1] ← -3.
				A[ROW+1,COL-2] ← -2.
				A[ROW+2,COL+2] ← (A[ROW+1,COL+1] ← TAU[SEG-1]
				   /TAU[SEG])↑2;
			ROW ← ROW + 3;
			COL ← COL + 4;
			ALOC ← ALOC + N + 4;
			END

			ELSE BEGIN  "CNST" COMMENT:  Left of segment is a
			constrained point;
			MEM(ALOC-1) ← MEM(ALOC-2) ← MEM(ALOC-3) ← MEM(ALOC-4)
				← MEM(ALOC+N) ← 1.;
			ALOC ← ALOC + N + N;
			MEM(ALOC-1) ← -3;
			MEM(ALOC-2) ← -2;
			MEM(ALOC-3) ← -1;
			MEM(ALOC+1) ← TEMP ← TAU[SEG-1] / TAU[SEG];
			ALOC ← ALOC + N;
			MEM(ALOC-1) ← -3;
			MEM(ALOC-2) ← -1;
			MEM(ALOC+2) ← TEMP*TEMP;
			COMMENT:  equivalent to:
				A[ROW,COL-1] ← A[ROW,COL-2]
				  ← A[ROW,COL-3] ← A[ROW,COL-4] ← A[ROW+1,COL] ← 1.
				A[ROW+2,COL-3] ← A[ROW+3,COL-2] ← -1.
				A[ROW+2,COL-1] ← A[ROW+3,COL-1] ← -3.
				A[ROW+2,COL-2] ← -2.
				A[ROW+3,COL+2] ← (A[ROW+2,COL+1] ← TAU[SEG-1] /
					TAU[SEG])↑2;
			ROW ← ROW + 4;
			COL ← COL + 4;
			ALOC ← ALOC + N + 4;
			END;

	COMMENT:  Take care of the constraints at the final point;
	COL ← COL - 4;
	MEM(ALOC-4) ← MEM(ALOC-3) ← MEM(ALOC-2) ← MEM(ALOC-1) ← 1;
	ALOC ← ALOC + N;
	MEM(ALOC-3) ← 1.;
	MEM(ALOC-2) ← 2.;
	MEM(ALOC-1) ← 3.;
	ALOC ← ALOC + N;
	MEM(ALOC-2) ← 2.;
	MEM(ALOC-1) ← 6.;
	COMMENT:  equivalent to:
		A[ROW,COL] ← A[ROW,COL+1] ← A[ROW,COL+2] ← A[ROW,COL+3]  
			← A[ROW+1,COL+1] ← 1.
		A[ROW+1,COL+2] ← A[ROW+2,COL+2] ← 2.
		A[ROW+1,COL+3] ← 3.
		A[ROW+2,COL+3] ← 6.;
	ROW ← ROW + 3;
	COL ← COL + 4;

	IF ROW ≠ COL ∨ ROW ≠ N + 1 THEN OUTSTR("ERROR IN POLY"&CRLF);
	DECOMPOSE(N,A,A);
	END "polya";

PROCEDURE POLYB (REAL ARRAY COEFFS, A; INTEGER #SEGS; REAL ARRAY LOCS;
    INTEGER ARRAY TAU; REAL FST_VEL, LST_VEL, FST_ACC, LST_ACC;
    INTEGER UNC1, UNC2);

	BEGIN "polyb"

	DEFINE MEM(ARG) "<>" = <MEMORY[ARG,REAL]>;
	SAFE REAL ARRAY B [1:4*#SEGS];
	COMMENT:  A is the large matrix and B is the vector.
	We will solve Ax=B;
	INTEGER ROW, SEG, N, ALOC;
	REAL TEMP;

	ARRCLR(B);
	N ← 4 * #SEGS;
	UNC1 ← UNC1 + 1;
	UNC2 ← UNC2 + 1;
	ROW ← 1;
	SEG ← 1;

	COMMENT:  Fill the matrix for the first segment;
	B[ROW] ← LOCS[0];
	B[ROW+1] ← TAU[SEG] * FST_VEL;
	B[ROW+2] ← TAU[SEG]↑2 * FST_ACC;
	ROW ← ROW + 3;

	FOR SEG ← 2 STEP 1 UNTIL #SEGS DO
		IF SEG=UNC1 ∨ SEG=UNC2 
			THEN BEGIN  "UNCNST" COMMENT:  Left of segment
			is an unconstrained point;
			ROW ← ROW + 3;
			END

			ELSE BEGIN  "CNST" COMMENT:  Left of segment is a
			constrained point;
			B[ROW] ← B[ROW+1] ← LOCS[SEG-1];
			ROW ← ROW + 4;
			END;

	COMMENT:  Take care of the constraints at the final point;
	B[ROW] ← LOCS[#SEGS];
	B[ROW+1] ← TAU[#SEGS] * LST_VEL;
	B[ROW+2] ← TAU[#SEGS]↑2 * LST_ACC;
	ROW ← ROW + 3;

	IF ROW ≠ N + 1 THEN OUTSTR("ERROR IN POLY"&CRLF);
	SOLVE(N,A,B,COEFFS);
	END "polyb"